/*2.0版本搜线函数，两次搜线，暂时不稳定，不提倡使用*/
void AdvancedRegression(int type, int startline1, int endline1, int startline2, int endline2)
{
	int i = 0;
	int sumlines1 = endline1 - startline1;
	int sumlines2 = endline2 - startline2;
	int sumX = 0;
	int sumY = 0;
	float averageX = 0;
	float averageY = 0;
	float sumUp = 0;
	float sumDown = 0;
	if (type == 0)  //拟合中线
	{
		/**计算sumX sumY**/
		for (i = startline1; i < endline1; i++)
		{
			sumX += i;
			sumY += LeftCenter[i];
		}
		for (i = startline2; i < endline2; i++)
		{
			sumX += i;
			sumY += LeftCenter[i];
		}
		averageX = (float)(sumX / (sumlines1 + sumlines2));     //x的平均值
		averageY = (float)(sumY / (sumlines1 + sumlines2));     //y的平均值
		for (i = startline1; i < endline1; i++)
		{
			sumUp += (LeftCenter[i] - averageY) * (i - averageX);
			sumDown += (i - averageX) * (i - averageX);
		}
		for (i = startline2; i < endline2; i++)
		{
			sumUp += (LeftCenter[i] - averageY) * (i - averageX);
			sumDown += (i - averageX) * (i - averageX);
		}
		if (sumDown == 0)
			parameterB = 0;
		else
			parameterB = sumUp / sumDown;
		parameterA = averageY - parameterB * averageX;
	}
	else if (type == 1)     //拟合左线
	{
		/**计算sumX sumY**/
		for (i = startline1; i < endline1; i++)
		{
			sumX += i;
			sumY += LeftBlack[i];
		}
		for (i = startline2; i < endline2; i++)
		{
			sumX += i;
			sumY += LeftBlack[i];
		}
		averageX = (float)(sumX / (sumlines1 + sumlines2));     //x的平均值
		averageY = (float)(sumY / (sumlines1 + sumlines2));     //y的平均值
		for (i = startline1; i < endline1; i++)
		{
			sumUp += (LeftBlack[i] - averageY) * (i - averageX);
			sumDown += (i - averageX) * (i - averageX);
		}
		for (i = startline2; i < endline2; i++)
		{
			sumUp += (LeftBlack[i] - averageY) * (i - averageX);
			sumDown += (i - averageX) * (i - averageX);
		}
		if (sumDown == 0)
			parameterB = 0;
		else
			parameterB = sumUp / sumDown;
		parameterA = averageY - parameterB * averageX;
	}
	else if (type == 2)         //拟合右线
	{
		/**计算sumX sumY**/
		for (i = startline1; i < endline1; i++)
		{
			sumX += i;
			sumY += RightBlack[i];
		}
		for (i = startline2; i < endline2; i++)
		{
			sumX += i;
			sumY += RightBlack[i];
		}
		averageX = (float)(sumX / (sumlines1 + sumlines2));     //x的平均值
		averageY = (float)(sumY / (sumlines1 + sumlines2));     //y的平均值
		for (i = startline1; i < endline1; i++)
		{
			sumUp += (RightBlack[i] - averageY) * (i - averageX);
			sumDown += (i - averageX) * (i - averageX);
		}
		for (i = startline2; i < endline2; i++)
		{
			sumUp += (RightBlack[i] - averageY) * (i - averageX);
			sumDown += (i - averageX) * (i - averageX);
		}
		if (sumDown == 0)
			parameterB = 0;
		else
			parameterB = sumUp / sumDown;
		parameterA = averageY - parameterB * averageX;
	}

}
void ValueToMimeticMiddleLine(float A, float B, int start_point, int end_point)
{
	int m;
	for (m = start_point; m <= end_point; m++)
	{
		if ((B * m + A) >= 255)
			MimeticMiddleLine[m] = 255;
		if ((B * m + A) <= 0)
			MimeticMiddleLine[m] = 0;
		else if (0 < (B * m + A) && (B * m + A) < 255)
			MimeticMiddleLine[m] = B * m + A;
	}
}
void ValueToMimeticLeftLine(float A, float B, int start_point, int end_point)
{
	int m;
	for (m = start_point; m <= end_point; m++)
	{
		if ((B * m + A) >= 255)
			MimeticLeftLine[m] = 255;
		if ((B * m + A) <= 0)
			MimeticLeftLine[m] = 0;
		else if (0 < (B * m + A) && (B * m + A) < 255)
			MimeticLeftLine[m] = (int)(B * m + A);
	}
}
void ValueToMimeticRightLine(float A, float B, int start_point, int end_point)
{
	int m;
	for (m = start_point; m <= end_point; m++)
	{
		if ((B * m + A) >= 255)
			MimeticRightLine[m] = 255;
		if ((B * m + A) <= 0)
			MimeticRightLine[m] = 0;
		else if (0 < (B * m + A) && (B * m + A) < 255)
			MimeticRightLine[m] = (int)(B * m + A);
	}
}
void Regression(int type, int startline, int endline)//最小二乘法拟合曲线，分别拟合中线，左线，右线,type表示拟合哪几条线   xy 颠倒
{
	int i = 0;
	int sumlines = endline - startline;
	int sumX = 0;
	int sumY = 0;
	float averageX = 0;
	float averageY = 0;
	float sumUp = 0;
	float sumDown = 0;
	if (type == 0)      //拟合中线
	{
		for (i = startline; i < endline; i++)
		{
			sumX += i;
			sumY += LeftCenter[i];
		}
		if (sumlines != 0)
		{
			averageX = (float)(sumX / sumlines);     //x的平均值
			averageY = (float)(sumY / sumlines);     //y的平均值
		}
		else
		{
			averageX = 0;     //x的平均值
			averageY = 0;     //y的平均值
		}
		for (i = startline; i < endline; i++)
		{
			sumUp += (LeftCenter[i] - averageY) * (i - averageX);
			sumDown += (i - averageX) * (i - averageX);
		}
		if (sumDown == 0) parameterB = 0;
		else parameterB = sumUp / sumDown;
		parameterA = averageY - parameterB * averageX;
	}
	else if (type == 1)//拟合左线
	{
		for (i = startline; i < endline; i++)
		{
			sumX += i;
			sumY += LeftBlack[i];
		}
		if (sumlines == 0) sumlines = 1;
		averageX = (float)(sumX / sumlines);     //x的平均值
		averageY = (float)(sumY / sumlines);     //y的平均值
		for (i = startline; i < endline; i++)
		{
			sumUp += (LeftBlack[i] - averageY) * (i - averageX);
			sumDown += (i - averageX) * (i - averageX);
		}
		if (sumDown == 0) parameterB = 0;
		else parameterB = sumUp / sumDown;
		parameterA = averageY - parameterB * averageX;
	}
	else if (type == 2)//拟合右线
	{
		for (i = startline; i < endline; i++)
		{
			sumX += i;
			sumY += RightBlack[i];
		}
		if (sumlines == 0) sumlines = 1;
		averageX = (float)(sumX / sumlines);     //x的平均值
		averageY = (float)(sumY / sumlines);     //y的平均值
		for (i = startline; i < endline; i++)
		{
			sumUp += (RightBlack[i] - averageY) * (i - averageX);
			sumDown += (i - averageX) * (i - averageX);
		}
		if (sumDown == 0) parameterB = 0;
		else parameterB = sumUp / sumDown;
		parameterA = averageY - parameterB * averageX;
	}
}
void SquareFeet(int begin, int end, int type)
{
	int i = 0;
	if (type == 1)//左线拟合差平方计算
	{
		int SquareLeft = 0;
		Regression(1, begin, end);
		ValueToMimeticLeftLine(parameterA, parameterB, (int)begin, (int)end);
		for (i = begin; i <= end; i++)
		{
			SquareLeft = SquareLeft + (LeftBlack[i] - MimeticLineLeft[i]) * (LeftBlack[i] - MimeticLineLeft[i]);
		}
		SquareLeft = SquareLeft / (end - begin + 1);
	}
	if (type == 2)//右线拟合差平方计算
	{
		int pianfangright = 0;
		Regression(2, begin, end);
		ValueToMimeticRightLine(parameterA, parameterB, (int)begin, (int)end);
		for (i = begin; i <= end; i++)
		{
			pianfangright = pianfangright + (RightBlack[i] - MimeticRightLine[i]) * (RightBlack[i] - MimeticRightLine[i]);
		}
		pianfangright = pianfangright / (end - begin + 1);
	}
	if (type == 0)//中线拟合差平方计算
	{
		int pianfangmid = 0;
		Regression(0, begin, end);
		ValueToMimeticMiddleLine(parameterA, parameterB, (int)begin, (int)end);
		int fangjun = 0;
		int junfang = 0;
		for (i = begin; i <= end; i++)
		{
			fangjun = fangjun + (LeftCenter[i]) * (LeftCenter[i]);
		}
		fangjun = fangjun / (end - begin + 1);
		for (i = begin; i <= end; i++)
		{
			junfang = junfang + (LeftCenter[i]);
		}
		junfang = junfang / (end - begin + 1);
		junfang = junfang * junfang;
		pianfangmid = fangjun - junfang;
	}

}
void SearchLineTwice()
{
	IMAGE_MIDDLE_SET();  // 获取动态阈值

	// 获得二值化图像
	for (int i = 0; i < MAX_VIDEO_LINE; i++)
	{
		for (int j = 0; j < MAX_VIDEO_POINT; j++)
		{
			if (g_VideoImageData[i][j] >= IMAGE_MIDDLE)
				tmp[i][j] = 1;
			else
				tmp[i][j] = 0;
		}
	}
	// 图像翻转
	for (int i = 0; i < MAX_VIDEO_LINE; i++)
		for (int j = 0; j < MAX_VIDEO_POINT; j++)
			g_VideoImageData[i][j] = tmp[i][MAX_VIDEO_POINT - j - 1];

	// 第一次搜线
	for (int row = 0; row < sum_num_row; row++)
	{
		// 右边线
		for (int col = MiddleLineOld; col >= 1; col--)
		{
			if (col >= MAX_VIDEO_POINT)
				col = MAX_VIDEO_POINT;
			if (g_VideoImageData[row][col - 1] == 1 && (g_VideoImageData[row][col] == 0 || g_VideoImageData[row][col] == 1) && g_VideoImageData[row][col + 1] == 0) //黑白
			{
				RightBlack[row] = col + 1;
				RightOld = col + 1;
				RightFlag[row] = 1;
				g_RightEdge[row].x = row;
				g_RightEdge[row].y = col;
				break;
			}
			else
			{
				RightBlack[row] = 0;
				RightFlag[row] = 0;
			}
		}

		// 左边线
		for (int col = MiddleLineOld; col < MAX_VIDEO_POINT; col++)
		{
			if (col == MAX_VIDEO_POINT)
				col = MAX_VIDEO_POINT;
			if (g_VideoImageData[row][col + 1] == 0 && (g_VideoImageData[row][col] == 1 || g_VideoImageData[row][col] == 0) && g_VideoImageData[row][col - 1] == 1)
			{

				LeftBlack[row] = col - 1;
				LeftOld = col - 1;
				LeftFlag[row] = 1;
				g_LeftEdge[row].x = row;
				g_LeftEdge[row].y = col;
				break;
			}
			else
			{
				LeftBlack[row] = MAX_VIDEO_POINT;
				LeftFlag[row] = 0;
			}
		}
		//下一行的扫线在上一行的基础上向左右扫
		MiddleLineOld = (LeftBlack[row] + RightBlack[row]) / 2;
	}
	// 中心线
	for (int i = 0; i < sum_num_row; i++)
	{
		g_CenterPosition[i].x = i;
		g_CenterPosition[i].y = (LeftBlack[i] + RightBlack[i]) / 2;
	}

	// 二次搜线
	for (int i = 1; i < sum_num_row; i++)  //扫断点0
	{
		if ((LeftBlack[i] - RightBlack[i]) <= (LeftBlack[i - 1] - RightBlack[i - 1])) {};
		if ((LeftBlack[i] - RightBlack[i]) - (LeftBlack[i - 1] - RightBlack[i - 1]) >= 4 && (i >= 13))
		{
			LackLine0 = i - 1;
			LackLineDistance = LeftBlack[i - 1] - RightBlack[i - 1];
			break;
		}
	}
	if (LackLine0 > 10)
	{
		AdvancedRegression(0, LackLine0 - 9, LackLine0 - 7, LackLine0 - 5, LackLine0 - 3); //显示断行处斜率
		ValueToMimeticMiddleLine(parameterA, parameterB, 0, 69);
		for (int row = LackLine0 + 3; row < sum_num_row; row++)  //固定中值扫线
		{
			for (int col = MimeticMiddleLine[row]; col >= 1; col--)  //扫右线
			{
				if (col <= 0)
					col = 0;
				if (g_VideoImageData[row][col - 1] == 1 && g_VideoImageData[row][col] == 0 && g_VideoImageData[row][col + 1] == 0) //黑黑白
				{

					RightBlack[row] = col + 1;
					RightOld = col + 1;
					RightFlag[row] = 1;
					break;
				}
				else
				{
					RightBlack[row] = 0;
					RightFlag[row] = 0;
				}
			}
			for (int col = MimeticMiddleLine[row]; col < MAX_VIDEO_POINT; col++)  //扫左线
			{
				if (col >= MAX_VIDEO_POINT)
					col = MAX_VIDEO_POINT;
				if (g_VideoImageData[row][col + 1] == 0 && g_VideoImageData[row][col] == 0 && g_VideoImageData[row][col - 1] == 1)
				{
					LeftBlack[row] = col - 1;
					LeftOld = col - 1;
					LeftFlag[row] = 1;
					break;
				}
				else
				{
					LeftBlack[row] = MAX_VIDEO_POINT;
					LeftFlag[row] = 0;
				}
			}
		}
	}
	for (int row = LackLine0 + 3; row < sum_num_row; row++)//扫断点1
	{
		if ((LeftBlack[row] - RightBlack[row]) < LackLineDistance - 5)
		{
			LackLine1 = row;
			break;
		}
	}
	if (LackLine0 > 10 && LackLine1 != 0)
	{
		MiddleLineTmp = MimeticMiddleLine[LackLine1];
		int gudingtime = 1;
		for (int row = LackLine1; row < sum_num_row; row++)  //二次扫线
		{
			for (int col = MiddleLineTmp; col >= 1; col--)  //扫左线
			{
				if (col >= MAX_VIDEO_POINT)
					col = MAX_VIDEO_POINT;
				if (g_VideoImageData[row][col - 1] == 1 && g_VideoImageData[row][col] == 0 && g_VideoImageData[row][col + 1] == 0) //黑黑白
				{

					RightBlack[row] = col + 1;
					RightOld = col + 1;
					RightFlag[row] = 1;
					break;
				}
				else
				{
					RightBlack[row] = 0;
					RightFlag[row] = 0;
				}
			}
			for (int col = MiddleLineTmp; col < MAX_VIDEO_POINT; col++)  //扫左线
			{
				if (col == 0)
					col = 1;
				if (g_VideoImageData[row][col + 1] == 0 && g_VideoImageData[row][col] == 0 && g_VideoImageData[row][col - 1] == 1)
				{
					LeftBlack[row] = col - 1;
					LeftOld = col - 1;
					LeftFlag[row] = 1;
					break;
				}
				else
				{
					LeftBlack[row] = MAX_VIDEO_POINT;
					LeftFlag[row] = 0;
				}
				MiddleLineTmp = (LeftFlag[row] + RightBlack[row]) / 2;
			}
		}

		//终止,存入显示数组中
		int row, col;
		for (row = 0; row < MAX_VIDEO_LINE; row++)  //去掉杂点
		{
			LeftCenter[row] = (LeftBlack[row] + RightBlack[row]) / 2;
			if (row > 5)
			{
				if (g_VideoImageData[row][LeftCenter[row]] == 0 && g_VideoImageData[row + 1][LeftCenter[row]] == 0 && RoundaountMemory != 4 && RoundaountMemoryLeft != 4)
				{
					for (int WheelLine = row; WheelLine < MAX_VIDEO_LINE; WheelLine++)
					{
						LeftCenter[row] = LeftCenter[row - 1];
						LeftBlack[WheelLine] = 0;
						RightBlack[WheelLine] = 0;
					}
					break;
				}
				g_CenterPosition[row].x = LeftCenter[row];
			}
		}
		int BreakLineNums = 0, j;
		for (j = 0; j < 50; j++)
		{
			if ((g_VideoImageData[j][LeftCenter[j]]) == 0 && (g_VideoImageData[j + 1][LeftCenter[j]]) == 0 && RoundaountMemory != 4 && RoundaountMemoryLeft != 4)
			{
				BreakLineNums = j;
				if (BreakLineNums >= 20)    //防止在一开始就break
				{
					break;
				}
			}
			if ((g_VideoImageData[j][LeftCenter[j]]) == 0 && (g_VideoImageData[j + 1][LeftCenter[j + 1]]) == 0 && (g_VideoImageData[j + 2][LeftCenter[j + 2]]) == 0 && (g_VideoImageData[j + 3][LeftCenter[j + 3]]) == 0 && (RoundaountMemory == 4 || RoundaountMemoryLeft == 4))
			{
				BreakLineNums = j;
				if (BreakLineNums >= 20)    //防止在一开始就break
				{
					break;
				}
			}
		}
		if (BreakLineNums == 0)
			BreakLineNums = 50;
		MiddleLineOld = LeftCenter[4];
		if (BreakLineNums >= 4)
		{
			int calendleft = 0;
			int calendright = 0;
			int i;
			for (i = BreakLineNums - 1; i >= 4; i--)
			{
				if (ABS((LeftBlack[i] - LeftBlack[i - 1])) <= 4 && LeftBlack[i] != MAX_VIDEO_POINT)
				{
					//计算左边线的最终有效行数
					calendleft = i - 1;
					break;
				}
			}
			for (i = calendleft; i >= 4; i--)
			{
				if (LeftBlack[i] == LeftBlack[i - 1])
					calendleft = i - 1;
				if (LeftBlack[i] != LeftBlack[i - 1])
					break;
			}
			for (i = BreakLineNums - 1; i >= 4; i--)
			{
				if (ABS(RightBlack[i] - RightBlack[i - 1]) <= 4 && RightBlack[i] != 0)
				{
					//计算右边线的最终有效行数
					calendright = i - 1;
					break;
				}
			}
			for (i = calendright; i >= 4; i--)
			{
				if (RightBlack[i] == RightBlack[i - 1])
					calendright = i - 1;
				if (RightBlack[i] != RightBlack[i - 1])
					break;
			}
			if (calendleft >= 45)
				calendleft = 45;
			if (calendright >= 45)
				calendright = 45;
			if (calendleft >= 20)
			{
				SquareFeet(0, calendleft, 1);
			}
			if (calendleft < 20)
			{
				SquareFeet(0, BreakLineNums - 3, 1);
			}
			if (calendright >= 20)
			{
				SquareFeet(0, calendright, 2);
			}
			if (calendright < 20)
			{
				SquareFeet(0, BreakLineNums - 3, 2);
			}
		}
		int countprotect = 0;
		if (RampWayFlag == 0 && GoToGarage == 0)  //坡道不退出  入库刹车10帧后自行退出
		{
			for (j = 0; j <= 185; j++)  //退出
			{
				if ((g_VideoImageData[0][j]) == 0)
				{
					countprotect = countprotect + 1;
				}
				if (countprotect == 186)
				{
					protect = 1;
					break;
				}
			}
		}
	}
}